from cProfile import label
import math
from tempfile import tempdir
from turtle import position
import numpy as np
import copy
from Method import *


def Avoid_wall(last_pi, pi, wall, block_list, wall_list, ceilling):
    for i in range(len(wall_list)):
        if wall == wall_list[i]:
            wall_list.pop(i)
            break

    if last_pi == None:
        move_vector = calculate_move_vector(pi, wall)  # 获取避障方向集合，看看优先向那个方向避障
        for j in range(len(move_vector)):
            distance = wall[2] / 10
            move_step = [move_vector[j][i] * distance for i in range(3)]
            start_move_point = pi[0]
            end_move_point = pi[1]
            while (1):
                start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
                end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
                # todo 添加pi[5]
                new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4], '避障成功']

                distance_vector = [pi[0][i] - start_move_point[i] for i in range(3)]
                move_distance = math.sqrt(sum(e ** 2 for e in distance_vector))

                is_collision = collision_detection(new_pi, wall)
                if is_collision:
                    continue

                is_collision1 = False
                for i in range(len(wall_list)):
                    temp = collision_detection(new_pi, wall_list[i])
                    if temp:
                        cos_value = get_cos_value(new_pi, wall_list[i])
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision1 = True
                            break

                if is_collision1:
                    if j == 1:
                        continue
                    else:
                        break

                if move_distance > 1000:
                    if j == 1:
                        print('移出墙体失败')
                        # todo 这里可以修改墙体避障失败
                        pi[5] = "避障失败"
                        return [pi, [0, 0, 0], None, pi[1]]
                    else:
                        break

                rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
                print("墙体避障成功")
                return [new_pi, rectify, None, None]

    else:
        # 得先判断 上一条管道方向是不是竖直方向的
        cos_value = get_cos_value(last_pi, [[0, 0, 0], [0, 0, 1]])
        print(cos_value)
        if 0.45 <= cos_value <= 1 or -1 <= cos_value <= -0.45:  # 如果上一条管道是垂直方向
            move_vector = calculate_move_vector(pi, wall)  # 获取避障方向集合，看看优先向那个方向避障
            for j in range(len(move_vector)):
                distance = wall[2] / 10
                move_step = [move_vector[j][i] * distance for i in range(3)]
                start_move_point = pi[0]
                end_move_point = pi[1]
                while (1):
                    start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
                    end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
                    # todo 添加pi[5]
                    new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4], '避障成功']

                    distance_vector = [pi[0][i] - start_move_point[i] for i in range(3)]
                    move_distance = math.sqrt(sum(e ** 2 for e in distance_vector))

                    copy_new_pi = copy.deepcopy(new_pi)
                    copy_new_pi[2], copy_new_pi[3] = copy_new_pi[2] + 30, copy_new_pi[3] + 30
                    is_collision = collision_detection(copy_new_pi, wall)
                    if is_collision:
                        continue

                    is_collision1 = False
                    for i in range(len(wall_list)):
                        temp = collision_detection(copy_new_pi, wall_list[i])
                        if temp:
                            cos_value = get_cos_value(copy_new_pi, wall_list[i])
                            if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                                is_collision1 = True
                                break

                    if is_collision1:
                        if j == 1:
                            continue
                        else:
                            break

                    # 判断是否和block发生碰撞
                    for block in block_list:
                        if collision_detection(copy_new_pi, block):
                            continue

                    if move_distance > 1000:
                        if j == 1:
                            print('移出墙体失败')
                            # todo 这里可以修改墙体避障失败
                            pi[5] = "避障失败"
                            return [pi, [0, 0, 0], None, pi[1]]
                        else:
                            break

                    # 更新上一条管道
                    start_new_last_pi = [last_pi[0][i] + move_step[i] for i in range(3)]
                    end_new_last_pi = [last_pi[1][i] + move_step[i] for i in range(3)]
                    new_last_pi = [start_new_last_pi, end_new_last_pi, last_pi[2], last_pi[3], last_pi[4], '避障成功']
                    new_pi[5] = '避障成功'
                    rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
                    print("墙体避障成功", new_pi)
                    return [new_pi, rectify, new_last_pi, None]

        else:  # 上一条管道与墙是垂直方向
            lengthen_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
            shorten_vector = list(map(lambda x, y: x - y, last_pi[0], last_pi[1]))
            unit_lengthen_vector = get_Unit_vector(lengthen_vector)
            unit_shorten_vector = get_Unit_vector(shorten_vector)
            move_vector = [unit_shorten_vector, unit_lengthen_vector]  # 先尝试缩短，再尝试延长

            for j in range(len(move_vector)):
                distance = wall[2] / 10
                move_step = [move_vector[j][i] * distance for i in range(3)]
                move_point = last_pi[1]
                while (1):
                    move_point = [move_point[i] + move_step[i] for i in range(3)]
                    move_rectify = [move_point[i] - last_pi[1][i] for i in range(3)]
                    move_distance = math.sqrt(sum(e ** 2 for e in move_rectify))
                    if j == 0:
                        last_pi_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
                        backward_max = np.dot(last_pi[0], last_pi_vector)  # 最大回退点
                        backward_value = np.dot(move_point, last_pi_vector)  # 回退点
                        if backward_value < backward_max:  # 若回退点超出最大回退点
                            break
                    # todo 添加pi[5]
                    new_pi = [[pi[0][i] + move_rectify[i] for i in range(3)],
                              [pi[1][i] + move_rectify[i] for i in range(3)], pi[2], pi[3], pi[4], '避障成功']  # 移动后的管道信息

                    is_collision = collision_detection(new_pi, wall)
                    if is_collision:
                        continue

                    is_collision1 = False
                    for i in range(len(wall_list)):
                        if collision_detection(new_pi, wall_list[i]):
                            cos_value = get_cos_value(new_pi, wall_list[i])
                            if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                                is_collision1 = True
                                break

                    is_collision2 = False
                    # 还需要判断是否和其他block是否发生碰撞
                    for block in block_list:
                        if collision_detection(new_pi, block):
                            cos_value = get_cos_value(new_pi, block)
                            is_collision2 = True
                            break

                    if is_collision1 or is_collision2:
                        if j == 0:
                            break
                        else:
                            continue

                    if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0] or new_pi[0][2] >= ceilling[1] or \
                            new_pi[1][2] >= ceilling[1]:
                        if j == 0:
                            break
                        else:
                            print('移出墙体失败')
                            pi[5] = "避障失败"
                            return [pi, [0, 0, 0], None, pi[1]]

                    if move_distance > 1000:
                        if j == 0:
                            break
                        else:
                            print('移出墙体失败')
                            pi[5] = "避障失败"
                            return [pi, [0, 0, 0], None, pi[1]]

                    new_last_pi_end = [last_pi[1][i] + move_rectify[i] for i in range(3)]
                    # todo 添加last_pi[5]
                    new_last_pi = [last_pi[0], new_last_pi_end, last_pi[2], last_pi[3], last_pi[4], '避障成功']
                    x1 = pi[1][0]
                    y1 = pi[1][1]
                    z1 = pi[1][2]
                    x2 = new_pi[1][0]
                    y2 = new_pi[1][1]
                    z2 = new_pi[1][2]
                    rectify = [x2 - x1, y2 - y1, z2 - z1]
                    print("墙体避障成功")
                    new_pi[5] = '避障成功'
                    return [new_pi, rectify, new_last_pi, None]


def Avoid_move(last_pi, pi, block, block_list, PipeGroup_Type, BlockBase, wall_list, initial_rectify, ceilling,
               endpipe_flag, stored_newpi_rectify, stored_position):
    from Avoidance_CURD import Pi_bypass
    print('进入移动避障', last_pi)
    print("pi", pi)
    stored_position2 = None
    recursion_position = None
    # 为什么判断 last_pi 上一段管道
    if last_pi is None:  # 判断是否为第一条管道
        move_vector = [0, 0, -1]  # 为什么是z轴方向
        distance = block[3] / 4
        move_step = [move_vector[i] * distance for i in range(3)]  # 移动的步数为障碍物宽的1/4
        start_move_point = pi[0]
        end_move_point = pi[1]
        # 为什么一开始进行向下避障
        print("为第一段管道，先进行向下避障")
        while (1):
            start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
            end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
            # todo cmf2023-12-7 添加pi[5]
            new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4], '避障成功']

            is_collision = collision_detection(new_pi, block)
            if is_collision:
                continue

            is_collision1 = False
            for i in range(len(block_list)):
                temp = collision_detection(new_pi, block_list[i])  # 判断避障后的管道是否与其他障碍物发生碰撞
                if temp:
                    is_collision1 = True
                    break
            if is_collision1:
                continue

            move_distance = pi[0][2] - start_move_point[2]
            if move_distance > 2000:
                break

            rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
            rectify = [rectify[i] + initial_rectify[i] for i in range(3)]

            if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0]:  # 判断起点是否在天花板上
                stored_newpi = copy.deepcopy(new_pi)
                stored_rectify = copy.deepcopy(rectify)
                stored_position2 = new_pi[0]
                print("超过天花板高度")
                break

            return [[[new_pi], rectify], 'Done', None, None, [None, None], None]

        # 进行左右移动避障
        print("进行左右避障")
        move_vector = calculate_move_vector(pi, block)  # 获取避障方向集合，看看优先向那个方向避障

        for j in range(len(move_vector)):  # 进行左绕右绕避障尝试

            distance = block[2] / 3
            move_step = [move_vector[j][i] * distance for i in range(3)]  # 移动的距离
            start_move_point = pi[0]
            end_move_point = pi[1]
            while 1:
                start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]  # 起点进行移动
                end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]  # 终点进行移动
                # todo cmf2023-12-7 添加pi[5]
                new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4], '避障成功']  # 生成移动后的新管道

                distance_vector = [pi[0][i] - start_move_point[i] for i in range(3)]
                move_distance = math.sqrt(sum(e ** 2 for e in distance_vector))  # 计算移动的距离

                is_collision = collision_detection(new_pi, block)  # 判断是否发碰撞
                if is_collision:
                    continue

                is_collision2 = False
                for i in range(len(wall_list)):  # 判断新管道是否和墙发生碰撞
                    temp = collision_detection(new_pi, wall_list[i])
                    cos_value = get_cos_value(new_pi, wall_list[i])
                    if temp:
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision2 = True
                            break
                    print("没有和墙发生碰撞")

                for i in range(len(block_list)):  # 判断新管道和障碍物是否发生碰撞
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        print("和障碍物发生碰撞")
                        is_collision2 = True
                        break

                if j == 0 and is_collision2:
                    if move_distance < 500:
                        continue
                    else:
                        break

                if move_distance > 2000:  # 判断移动距离是否超过 设定 长度
                    print("move_distance > 2000")
                    if j == 1:
                        if stored_position[0] is None:
                            if stored_position2 is None:
                                print('stored_position2 is None')
                                return [None, 'Fail', None, None, [None, pi[0]], None]
                            else:
                                return [[[stored_newpi], stored_rectify], 'Done', None, None, [stored_position2, None],
                                        None]
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None,
                                    [stored_position[0], None], None]
                    else:
                        break

                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        block_list_leave = copy.deepcopy(block_list)
                        block_list_leave.append(block)
                        change_flag, new_pi, recursion_position = Pi_bypass([new_pi], block_list, BlockBase,
                                                                            block_list_leave, PipeGroup_Type, ceilling)
                        break

                if len(new_pi[0]) == 3:  # 这个应该是看是不是只有一段管道
                    print("len(new_pi[0]) == 3，new_pi",new_pi)
                    rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    # 最终返回
                    return [[[new_pi], rectify], 'Done', None, None, [None, None], recursion_position]
                else:
                    rectify = [new_pi[-1][0][i] - pi[0][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[new_pi, rectify], 'Done', None, None, [None, None], recursion_position]

    else:  # 不是第一段管道
        print(last_pi)
        lengthen_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
        shorten_vector = list(map(lambda x, y: x - y, last_pi[0], last_pi[1]))
        unit_lengthen_vector = get_Unit_vector(lengthen_vector)
        unit_shorten_vector = get_Unit_vector(shorten_vector)
        move_vector = [unit_lengthen_vector, unit_shorten_vector]
        move_vector = calculate_move_vector(pi, block)  # 获取避障方向集合，看看优先向那个方向避障

        for j in range(len(move_vector)):
            distance = block[2] / 3
            move_step = [move_vector[j][i] * distance for i in range(3)]
            move_point = last_pi[1]
            while (1):
                move_point = [move_point[i] + move_step[i] for i in range(3)]
                move_rectify = [move_point[i] - last_pi[1][i] for i in range(3)]
                move_distance_xy = math.sqrt(move_rectify[0] ** 2 + move_rectify[1] ** 2)
                if j == 1:
                    last_pi_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
                    backward_max = np.dot(last_pi[0], last_pi_vector)  # 最大回退点
                    backward_value = np.dot(move_point, last_pi_vector)  # 回退点
                    if backward_value < backward_max:  # 若回退点超出最大回退点
                        if stored_position[0] is None:
                            print('1')
                            return [None, 'Fail', None, None, [None, pi[0]], None]  # 避障失败
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None,
                                    [stored_position[0], None], None]
                # todo cmf2023-12-7 添加pi[5]
                new_pi = [[pi[0][i] + move_rectify[i] for i in range(3)],
                          [pi[1][i] + move_rectify[i] for i in range(3)], pi[2], pi[3], pi[4], '避障成功']  # 移动后的管道信息

                is_collision = collision_detection(new_pi, block)
                if is_collision:
                    continue

                is_collision2 = False
                for i in range(len(wall_list)):
                    temp = collision_detection(new_pi, wall_list[i])
                    cos_value = get_cos_value(new_pi, wall_list[i])
                    if temp:
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision2 = True
                            break

                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        is_collision2 = True
                        break

                if j == 0 and is_collision2:
                    if move_distance_xy < 500:
                        continue
                    else:
                        break
                if get_Unit_vector(list(map(lambda x, y: x - y, last_pi[0], last_pi[1]))) == [0.0, 0.0, 1.0]:  #  判断 上一条管道是否是竖直管道， 1 是则 生成新管道和new_pi相接
                    new_pi_1_start = last_pi[1]
                    new_pi_1_end = new_pi[0]
                    new_pi_1 = [new_pi_1_start, new_pi_1_end, new_pi[2], new_pi[3], new_pi[4], '避障成功']
                    # 生成新管道
                    final_pi = [new_pi_1, new_pi]
                    print(final_pi)
                    is_collision3 = collision_detection(new_pi_1, block)
                    if is_collision3 and j == 0:
                        break
                    if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0] or new_pi[0][2] >= ceilling[1] or \
                            new_pi[1][2] >= ceilling[1]:  # 判断z点是否超过天花板限制
                        if j == 0:
                            break
                        else:
                            return [None, 'Fail', None, None, [None, pi[0]], None]

                    if move_distance_xy > 2000:
                        if stored_position[0] is None:
                            return [None, 'Fail', None, None, [None, pi[0]], None]
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None,
                                    [stored_position[0], None], None]
                    for i in range(len(block_list)):
                        temp = collision_detection(new_pi, block_list[i])
                        if temp:
                            block_list_leave = copy.deepcopy(block_list)
                            block_list_leave.append(block)
                            change_flag, final_pi, recursion_position = Pi_bypass(final_pi, block_list,
                                                                                BlockBase, block_list_leave,
                                                                                PipeGroup_Type,
                                                                                ceilling)

                    if len(new_pi[0]) == 3:
                        new_pi = [new_pi]
                    rectify = [new_pi[-1][1][i] - pi[1][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[final_pi, rectify], 'Done', None, last_pi, [None, None], recursion_position]
                else:
                    new_last_pi_end = [last_pi[1][i] + move_rectify[i] for i in range(3)]
                    # todo cmf2023-12-7 添加last_pi[5]
                    new_last_pi = [last_pi[0], new_last_pi_end, last_pi[2], last_pi[3], last_pi[4], '避障成功']
                    is_collision3 = collision_detection(new_last_pi, block)
                    if is_collision3 and j == 0:
                        break
                    if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0] or new_pi[0][2] >= ceilling[1] or \
                            new_pi[1][2] >= ceilling[1]:  # 判断z点是否超过天花板限制
                        if j == 0:
                            break
                        else:
                            print('2')
                            return [None, 'Fail', None, None, [None, pi[0]], None]

                    if move_distance_xy > 2000:
                        if stored_position[0] is None:
                            print('3')
                            return [None, 'Fail', None, None, [None, pi[0]], None]
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None,
                                    [stored_position[0], None], None]

                    for i in range(len(block_list)):
                        temp = collision_detection(new_pi, block_list[i])
                        if temp:
                            block_list_leave = copy.deepcopy(block_list)
                            block_list_leave.append(block)
                            change_flag, new_pi, recursion_position = Pi_bypass([new_last_pi, new_pi], block_list,
                                                                                BlockBase, block_list_leave,
                                                                                PipeGroup_Type,
                                                                                ceilling)
                            new_pi.pop(0)
                            break

                    if len(new_pi[0]) == 3:
                        new_pi = [new_pi]
                    rectify = [new_pi[-1][1][i] - pi[1][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    new_last_pi[1] = new_pi[0][0]
                    print(new_pi[0])
                    print(new_last_pi)
                    return [[new_pi, rectify], 'Done', None, new_last_pi, [None, None], recursion_position]


def Avoid_left_right(pi, block, block_list, BlockBase, wall_list, PipeGroup_Type, initial_rectify, ceilling,
                     Avoid_Column_flag):  # 左右避障
    from Avoidance_CURD import Pi_bypass
    print('进入左右避障')
    recursion_position = None
    intersection_points = get_Intersection_points(pi, block)  # 获取所有的碰撞坐标
    nearest_point = nearest_intersection_point(pi, intersection_points)  # 获取所有碰撞点中距离管道起点最短的点, 并投影到管道的方向向量上
    print("障碍物距离管道起点最近的点",nearest_point)
    pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
    block_vector = list(map(lambda x, y: x - y, block[1], block[0]))
    block_xy_length = math.sqrt(block_vector[0] ** 2 + block_vector[1] ** 2)

    try:
        slope = block_vector[2] / block_xy_length
    except:
        slope = float('inf')
    if slope < 0.3:
        # 当管道与障碍物夹角在75°-105°，视为垂直碰撞
        unit_pi_vector = get_Unit_vector_2D(pi_vector)
        unit_block_vector = get_Unit_vector_2D(block_vector)
        dot_result = np.dot(unit_pi_vector[0:2], unit_block_vector[0:2])

        if -0.258 <= dot_result <= 0.258:
            return [[[pi], initial_rectify], 'Change to up-down', None, None, [None, None], None]  # 若发生垂直碰撞，转为上下避障

    vector_length = math.sqrt(sum(e ** 2 for e in pi_vector))
    unit_pi_vector = [pi_vector[i] / vector_length for i in range(3)]  # 管道单位向量

    rotated_vector = rotation_2D(pi_vector, -90)  # 将管道方向向量顺时针旋转90度，用于右绕
    unit_rotated_vector = [rotated_vector[i] / vector_length for i in range(3)]
    reverse_rotated_vector = rotation_2D(pi_vector, 90)  ##将管道方向向量逆时针旋转90度，用于左绕
    unit_reverse_rotated_vector = [reverse_rotated_vector[i] / vector_length for i in range(3)]

    block_8point = block_dian2(block)  # 获取障碍物的8个点
    left_points = []
    right_points = []
    for i in range(len(block_8point)):
        vector = list(map(lambda x, y: x - y, block_8point[i], pi[0]))  # 向量：点减去线的起点
        left_or_right = multi_cross(pi_vector, vector)  # 若大于0，点在方向向量左边，反之在右边
        if left_or_right[2] > 0:
            left_points.append(block_8point[i])  # 在方向向量左边的点集合
        else:
            right_points.append(block_8point[i])  # 在方向向量右边的点集合
    print("左边点的集合", left_points)
    print("右边点的集合", right_points)
    if not left_points:
        move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
    elif not right_points:
        move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
    else:
        left_max_distance = distance_point_line(left_points[0], [pi[0], pi[1]])  # 左边的点中离直线距离最长的点
        for i in range(1, len(left_points)):
            temp = distance_point_line(left_points[i], [pi[0], pi[1]])
            if temp > left_max_distance:
                left_max_distance = temp

        right_max_distance = distance_point_line(right_points[0], [pi[0], pi[1]])  # 右边的点中离直线距离最长的点
        for i in range(1, len(right_points)):
            temp = distance_point_line(right_points[i], [pi[0], pi[1]])
            if temp > right_max_distance:
                right_max_distance = temp

        if right_max_distance >= left_max_distance:
            max_distance = left_max_distance
            move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
        else:
            max_distance = right_max_distance
            move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]

    for j in range(len(move_vector)):
        print("开始回退")
        change_direction = False
        backward_point = nearest_point  # 从管道最近的碰撞点开始回退
        while 1:
            if change_direction:
                break
            backward_point = get_Backward_point(backward_point, pi)  # 获取回退后的点

            vector1 = list(map(lambda x, y: x - y, pi[1], backward_point))
            distance1 = math.sqrt(sum(e ** 2 for e in vector1))  # 回退点到原管道终点的距离

            backward_max = np.dot(pi[0], pi_vector)  # 最大回退点
            backward_value = np.dot(backward_point, pi_vector)  # 回退点
            if backward_value < backward_max:  # 若回退点超出最大回退点
                print("超过最大回退点，进入移动避障", initial_rectify)
                information = Avoid_move(None, pi, block, block_list, PipeGroup_Type, BlockBase, wall_list,
                                         initial_rectify, ceilling, endpipe_flag=None, stored_newpi_rectify=None,
                                         stored_position=[None])
                print('消息', information)
                if information[1] != 'Fail':
                    return [[[pi], initial_rectify], 'Enter translational avoidance', None, None, [None, None], None]
                else:
                    print(1111)
                    return [[[pi], initial_rectify], 'Change to up-down', None, None, [None, None], None]
            # todo cmf 2023-12-7 添加 pi[5]
            new_pi1 = [pi[0], backward_point, pi[2], pi[3], None, '避障成功']

            horizontal_point = backward_point
            try:
                distance = max_distance / 3
            except:
                distance = pi[2] / 2
            horizontal_step = [move_vector[j][i] * distance for i in range(3)]
            leftright_distance = 0
            while (1):
                horizontal_point = [horizontal_point[i] + horizontal_step[i] for i in range(3)]
                leftright_distance += distance
                new_pi2_start = copy.deepcopy(new_pi1[1])
                # todo cmf 2023-12-7 添加 pi[5]
                new_pi2 = [new_pi2_start, horizontal_point, pi[2], pi[3], None, '避障成功']

                if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe':  # 若该段管道为水管，为生成的第二段管道设置斜率
                    len_pi = math.sqrt((new_pi2[1][0] - new_pi2[0][0]) ** 2 + (new_pi2[1][1] - new_pi2[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi2[1][2] = new_pi2[1][2] - step

                is_collision = collision_detection(new_pi1, block)
                is_collision1 = collision_detection(new_pi2, block)
                if is_collision or is_collision1:
                    break

                is_collision2 = False
                for i in range(len(wall_list)):
                    if collision_detection(new_pi2, wall_list[i]):
                        is_collision2 = True
                if is_collision2:  # 若生成的第二段管道碰上了本循环的障碍物和与原管道碰撞的wall，则继续回退
                    break

                step = [unit_pi_vector[i] * distance1 for i in range(3)]
                new_pi13_start = copy.deepcopy(new_pi2[1])
                new_pi3_end = copy.deepcopy(pi[1])
                new_pi3_end = [new_pi3_end[i] + leftright_distance*move_vector[j][i] for i in range(3)]
                # todo cmf 2023-12-7 添加 pi[5]
                new_pi3 = [new_pi13_start, new_pi3_end, pi[2], pi[3], None, '避障成功']

                if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe':  # 若该段管道为水管，为生成的第三段管道设置斜率
                    len_pi = math.sqrt((new_pi3[1][0] - new_pi3[0][0]) ** 2 + (new_pi3[1][1] - new_pi3[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi3[1][2] = new_pi3[1][2] - step

                # 为new_pi3添加安装空间
                copy_new_pi3 = copy.deepcopy(new_pi3)
                copy_new_pi3[2], copy_new_pi3[3] = copy_new_pi3[2] + 50, copy_new_pi3[3] + 50
                is_collision = collision_detection(copy_new_pi3, block)
                if is_collision:  # 如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
                    continue

                if leftright_distance <= pi[2] * 2.1:  # 2023-12-26 将 3 改成 1.5
                    continue

                final_pi = [new_pi1, new_pi2, new_pi3]
                for i in range(len(final_pi)):
                    # 判断是否和障碍物发生碰撞
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k])
                        if temp:
                            change_direction = True
                    # 判断是否和墙发生碰撞
                    for w in wall_list:
                        if collision_detection(final_pi[i], w):
                            print("和墙发生碰撞，转到移动避障")
                            return [[[pi], initial_rectify], 'Enter translational avoidance', None, None, [None, None],
                                    None]

                if change_direction:
                    if j == 1:
                        if Avoid_Column_flag:
                            block_list_leave = copy.deepcopy(block_list)
                            block_list_leave.append(block)
                            change_flag, final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase, block_list_leave,
                                                                     PipeGroup_Type, ceilling)
                        else:
                            return [[[pi], initial_rectify], 'Change to up-down', None, None, [None, None], None]
                    else:
                        break

                rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
                rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]


def Avoid_up_down(pi, block, block_list, BlockBase, wall_list, PipeGroup_Type, initial_rectify, ceilling):
    from Avoidance_CURD import Pi_bypass
    print('进入上下避障')
    recursion_position = None
    intersection_points = get_Intersection_points(pi, block)  # 获取所有的碰撞坐标
    nearest_point = nearest_intersection_point(pi, intersection_points)  # 获取所有碰撞点中距离管道起点最短的点并投影到管道的方向向量上
    high = block[3] / 5  # 上升或下降的距离
    change_direction = False
    backward_point = nearest_point  # 从管道最近的碰撞点开始回退
    while (1):
        if change_direction:
            backward_point = nearest_point
        backward_point = get_Backward_point(backward_point, pi)  # 获取回退后的点
        pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
        backward_max = np.dot(pi[0], pi_vector)  # 最大回退点
        backward_value = np.dot(backward_point, pi_vector)  # 回退点
        if backward_value < backward_max:  # 若回退点超出最大回退点
            print("回退点超出最大回退点")
            return [[[pi], initial_rectify], 'Enter translational avoidance', None, None, [None, None], None]

        # todo cmf 2023-12-7 添加 pi[5]
        new_pi1 = [pi[0], backward_point, pi[2], pi[3], None, '避障成功']

        downward_point_z = backward_point[2]
        updown_distance = 0
        move_vector = calculate_move_vector(pi, block)
        rotated_vector = rotation_2D(pi_vector, -90)  # 将管道方向向量顺时针旋转90度，用于右绕
        unit_rotated_vector = get_Unit_vector(rotated_vector)
        while (1):
            if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe' or PipeGroup_Type == 'Change to down':  # 若管道为水管或风管向上避障超过真天花板，则向下避障
                print(65)
                change_direction = False
                downward_point_z = downward_point_z - high
            else:  # 若管道为风管，向上或向下避障
                if unit_rotated_vector == move_vector[0]:  # 如果相等即为先左避障即为向下避障
                    print(11)
                    downward_point_z = downward_point_z + high
                else:
                    print(12)
                    downward_point_z = downward_point_z - high
            updown_distance += high
            downward_point = [new_pi1[1][0], new_pi1[1][1], downward_point_z]

            unit_pi_vector = get_Unit_vector(pi_vector)
            if unit_pi_vector[0] == 0:
                angle = math.pi / 2
            else:
                tan_value = unit_pi_vector[1] / unit_pi_vector[0]
                angle = math.atan(tan_value)
            if 0 < angle <= math.pi / 2:
                pi2_angle = angle - math.pi / 2
            else:
                pi2_angle = angle + math.pi / 2
            new_pi2_start = copy.deepcopy(new_pi1[1])
            # todo cmf 2023-12-7 添加 pi[5]
            new_pi2 = [new_pi2_start, downward_point, pi[2], pi[3], pi2_angle, '避障成功']

            is_collision1 = collision_detection(new_pi2, block)  # 继续回退
            if is_collision1:
                break

            is_collision2 = False
            for i in range(len(wall_list)):
                if collision_detection(new_pi2, wall_list[i]):
                    is_collision2 = True
            if is_collision2:  # 若生成的第二段管道碰上了本循环的障碍物和与原管道碰撞的wall，则继续回退
                break

            new_pi3_start = copy.deepcopy(new_pi2[1])
            # todo cmf 2023-12-7 添加 pi[5]
            new_pi3 = [new_pi3_start, [pi[1][0], pi[1][1], new_pi2[1][2] + pi_vector[2]], pi[2], pi[3], None, '避障成功']

            # if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe':  # 若该段管道为水管，为生成的第三段管道设置斜率
            #     len_pi = math.sqrt((new_pi3[1][0] - new_pi3[0][0]) ** 2 + (new_pi3[1][1] - new_pi3[0][1]) ** 2)
            #     step = len_pi / 100  # z坐标应该下降的高度
            #     new_pi3[1][2] = new_pi3[1][2] - step

            is_collision = collision_detection(new_pi3, block)  # 如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
            if is_collision:
                continue

            if updown_distance <= pi[3] * 2.5:  # 保证第二段管道长度满足生成fitting的条件
                continue

            final_pi = [new_pi1, new_pi2, new_pi3]
            print('当前上下避障成功')

            is_collision3 = False
            for i in range(len(final_pi)):
                for k in range(len(block_list)):
                    temp = collision_detection(final_pi[i], block_list[k])
                    if temp:
                        is_collision3 = True
                        break

            if is_collision3:
                block_list_leave = copy.deepcopy(block_list)
                block_list_leave.append(block)
                change_flag, final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase,
                                                                      block_list_leave, PipeGroup_Type,
                                                                      ceilling)  # 若生成的三段管道与其他障碍物发生碰撞，则继续调用避障函数

            # if PipeGroup_Type == 'Duct' or PipeGroup_Type == 'LineDuct':
            #     if new_pi2[0][2] >= ceilling[1] or new_pi2[1][2] >= ceilling[1]:
            #         change_direction = True
            #         PipeGroup_Type = 'Change to down'
            #
            #     for i in range(len(final_pi)):
            #         for k in range(len(block_list)):
            #             temp = collision_detection(final_pi[i], block_list[k])
            #             if temp:
            #                 change_direction = True
            #                 PipeGroup_Type = 'Change to down'
            #                 break
            #
            #     if change_direction:
            #         break

            if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe' or PipeGroup_Type == 'Change to down':
                print(PipeGroup_Type)
                if new_pi2[0][2] <= ceilling[0] or new_pi2[1][2] <= ceilling[0]:
                    rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1],
                               final_pi[-1][1][2] - pi[1][2]]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[[pi], initial_rectify], 'Enter translational avoidance', [final_pi, rectify], None,
                            [new_pi2[0], None], None]

                is_collision3 = False
                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k])
                        if temp:
                            is_collision3 = True
                            break

                if is_collision3:
                    print(222)
                    block_list_leave = copy.deepcopy(block_list)
                    block_list_leave.append(block)
                    change_flag, final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase,
                                                                          block_list_leave, PipeGroup_Type,
                                                                          ceilling)  # 若生成的三段管道与其他障碍物发生碰撞，则继续调用避障函数

            rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
            rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
            return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]


def pipe_preprocess(pi, block, cos_value, group_type):  # 若管道与障碍物夹角较小
    pipe_vector = [pi[1][i] - pi[0][i] for i in range(3)]
    pipe_length = math.sqrt(sum(e ** 2 for e in pipe_vector))
    block_vector = [block[1][i] - block[0][i] for i in range(3)]
    unit_block_vector = get_Unit_vector(block_vector)
    step = [unit_block_vector[i] * pipe_length for i in range(3)]
    if 0.966 < cos_value < 1:
        pi_end = [pi[0][i] + step[i] for i in range(3)]
    else:
        pi_end = [pi[0][i] - step[i] for i in range(3)]
    if group_type == 'Pipe' or group_type == 'LinePipe':  # 若该段管道为水管，设置斜率
        if pi_end[2] >= pi[0][2]:
            step = pipe_length / 100  # z坐标应该下降的高度
            pi_end[2] = pi_end[2] - step

    # todo cmf 2023-12-7 添加 pi[5]
    new_pi = [pi[0], pi_end, pi[2], pi[3], None, '避障成功']
    x1 = pi[1][0]
    y1 = pi[1][1]
    z1 = pi[1][2]
    x2 = new_pi[1][0]
    y2 = new_pi[1][1]
    z2 = new_pi[1][2]
    rectify = [x2 - x1, y2 - y1, z2 - z1]
    return [[[new_pi], rectify], 'Done', None, None, [None, None], None]


def Avoid_vertical(pi, block, block_list, BlockBase, PipeGroup_Type, initial_rectify, ceilling):
    from Avoidance_CURD import Pi_bypass

    recursion_position = None
    angle = pi[4]
    if angle is None:
        vector1 = [1, 0, 0]
    else:
        vector1 = [1, math.tan(angle), 0]
    vector1 = get_Unit_vector(vector1)
    vector2 = rotation_2D(vector1, 180)
    vector3 = rotation_2D(vector1, 90)
    vector4 = rotation_2D(vector1, -90)
    vector = [vector1, vector2, vector3, vector4]
    intersection_points = get_Intersection_points(pi, block)
    nearest_point = nearest_intersection_point(pi, intersection_points)
    for j in range(len(vector)):
        change_direction = False
        backward_point = nearest_point  # 从管道最近的碰撞点开始回退
        while (1):
            if change_direction:
                break
            backward_point = get_Backward_point_vertical(backward_point, pi)
            pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
            backward_max = np.dot(pi[0], pi_vector)  # 最大回退点
            backward_value = np.dot(backward_point, pi_vector)  # 回退点
            if backward_value < backward_max:  # 若回退点超出最大回退点，则返回原管道
                return [[[pi], [0, 0, 0]], 'Enter translational avoidance', None, None, [None, None], None]
            # todo cmf 2023-12-7 添加 pi[5]
            new_pi1 = [pi[0], backward_point, pi[2], pi[3], pi[4], '避障成功']

            horizontal_point = backward_point
            distance = block[2] / 3
            horizontal_step = [vector[j][i] * distance for i in range(3)]

            while (1):
                horizontal_point = [horizontal_point[i] + horizontal_step[i] for i in range(3)]
                if j == 0 or j == 1:
                    # todo cmf 2023-12-7 添加 pi[5]
                    new_pi2 = [new_pi1[1], horizontal_point, pi[3], pi[2], None, '避障成功']
                else:
                    # todo cmf 2023-12-7 添加 pi[5]
                    new_pi2 = [new_pi1[1], horizontal_point, pi[2], pi[3], None, '避障成功']

                if PipeGroup_Type == 'Pipe':  # 若该段管道为水管，为生成的第二段管道设置斜率
                    len_pi = math.sqrt((new_pi2[1][0] - new_pi2[0][0]) ** 2 + (new_pi2[1][1] - new_pi2[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi2[1][2] = new_pi2[1][2] - step

                is_collision = collision_detection(new_pi2, block)
                if is_collision:
                    break
                # todo cmf 2023-12-7 添加 pi[5]
                new_pi3 = [new_pi2[1], [new_pi2[1][0], new_pi2[1][1], pi[1][2]], pi[2], pi[3], pi[4], '避障成功']

                is_collision = collision_detection(new_pi3, block)  # 如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
                if is_collision:
                    continue

                final_pi = [new_pi1, new_pi2, new_pi3]

                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k])
                        if temp:
                            change_direction = True
                if change_direction:
                    if j == 3:
                        block_list_leave = copy.deepcopy(block_list)
                        block_list_leave.append(block)
                        final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase, block_list_leave,
                                                                 PipeGroup_Type,
                                                                 ceilling)  # 若生成的三段管道与其他障碍物发生碰撞，则继续调用避障函数
                    else:
                        break

                rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
                rectify = [rectify[i] + initial_rectify[i] for i in range(3)]

                return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]


# 获取需要进行避障的方向集合[先左或右]
def calculate_move_vector(pi, block):
    pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))

    rotated_vector = rotation_2D(pi_vector, -90)  # 将管道方向向量顺时针旋转90度，用于右绕
    unit_rotated_vector = get_Unit_vector(rotated_vector)
    reverse_rotated_vector = rotation_2D(pi_vector, 90)  # 将管道方向向量逆时针旋转90度，用于左绕
    unit_reverse_rotated_vector = get_Unit_vector(reverse_rotated_vector)

    block_8point = block_dian2(block)  # 获取障碍物的8个点
    left_points = []
    right_points = []

    for i in range(len(block_8point)):
        vector = list(map(lambda x, y: x - y, block_8point[i], pi[0]))  # 向量：点减去线的起点
        left_or_right = multi_cross(pi_vector, vector)  # 若大于0，点在方向向量左边，反之在右边

        if left_or_right[2] > 0:
            left_points.append(block_8point[i])  # 在方向向量左边的点集合
        else:
            right_points.append(block_8point[i])  # 在方向向量右边的点集合

    print("左边点的集合", left_points)
    print("右边点的集合", right_points)

    if not left_points:
        move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
    elif not right_points:
        move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
    else:
        left_max_distance = distance_point_line(left_points[0], [pi[0], pi[1]])  # 左边的点中离直线距离最长的点

        for i in range(1, len(left_points)):
            temp = distance_point_line(left_points[i], [pi[0], pi[1]])

            if temp > left_max_distance:
                left_max_distance = temp

        right_max_distance = distance_point_line(right_points[0], [pi[0], pi[1]])  # 右边的点中离直线距离最长的点

        for i in range(1, len(right_points)):
            temp = distance_point_line(right_points[i], [pi[0], pi[1]])

            if temp > right_max_distance:
                right_max_distance = temp

        if right_max_distance >= left_max_distance:
            move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
        else:
            move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
    if move_vector[0] == unit_reverse_rotated_vector:
        print("先向左进行避障")
    else:
        print("先向右移动避障")
    return move_vector